#ifndef CARTOGRAPHER_COMPLEMENTARY_FILTER_H
#define CARTOGRAPHER_COMPLEMENTARY_FILTER_H

#include "cartographer/rcm/common.h"

namespace cartographer
{
    class ComplementaryFilter
    {
    public:
        ComplementaryFilter();

        virtual ~ComplementaryFilter();

        bool setGainAcc(double gain);
        bool setGainMag(double gain);
        double getGainAcc() const;
        double getGainMag() const;
        bool setBiasAlpha(double bias_alpha);

        double getBiasAlpha() const;
        // When the filter is in the steady state, bias estimation will occur (if the parameter is enabled).
        bool getSteadyState() const;
        void setDoBiasEstimation(bool do_bias_estimation);
        bool getDoBiasEstimation() const;
        void setDoAdaptiveGain(bool do_adaptive_gain);
        bool getDoAdaptiveGain() const;
        double getAngularVelocityBiasX() const;
        double getAngularVelocityBiasY() const;
        double getAngularVelocityBiasZ() const;

        // Set the orientation, as a Hamilton Quaternion, of the body frame wrt the
        // fixed frame.
        void setOrientation(double q0, double q1, double q2, double q3);

        // Get the orientation, as a Hamilton Quaternion, of the body frame wrt the
        // fixed frame.
        void getOrientation(double &q0, double &q1, double &q2, double &q3) const;

        // Update from accelerometer and gyroscope data.
        // [ax, ay, az]: Normalized gravity vector.
        // [wx, wy, wz]: Angular veloctiy, in rad / s.
        // dt: time delta, in seconds.
        void update(const Eigen::Vector3d &acceleration, const Eigen::Vector3d &angular, double dt);

        // Update from accelerometer, gyroscope, and magnetometer data.
        // [ax, ay, az]: Normalized gravity vector.
        // [wx, wy, wz]: Angular veloctiy, in rad / s.
        // [mx, my, mz]: Magnetic field, units irrelevant.
        // dt: time delta, in seconds.
        void update(const Eigen::Vector3d &acceleration, const Eigen::Vector3d &angular, const Eigen::Vector3d &mag, double dt);

    private:
        static const double kGravity;
        static const double gamma_;
        // Bias estimation steady state thresholds
        static const double kAngularVelocityThreshold;
        static const double kAccelerationThreshold;
        static const double kDeltaAngularVelocityThreshold;

        // Gain parameter for the complementary filter, belongs in [0, 1].
        double gain_acc_;
        double gain_mag_;

        // Bias estimation gain parameter, belongs in [0, 1].
        double bias_alpha_;

        // Parameter whether to do bias estimation or not.
        bool do_bias_estimation_;

        // Parameter whether to do adaptive gain or not.
        bool do_adaptive_gain_;

        bool initialized_;
        bool steady_state_;

        // The orientation as a Hamilton quaternion (q0 is the scalar). Represents
        // the orientation of the fixed frame wrt the body frame.
        double q0_, q1_, q2_, q3_;

        // Bias in angular velocities;
        double wx_prev_, wy_prev_, wz_prev_;

        // Bias in angular velocities;
        double wx_bias_, wy_bias_, wz_bias_;

        void updateBiases(const Eigen::Vector3d &acceleration, const Eigen::Vector3d &angular);

        bool checkState(const Eigen::Vector3d &acceleration, const Eigen::Vector3d &angular) const;

        void getPrediction(double wx, double wy, double wz, double dt, double &q0_pred, double &q1_pred, double &q2_pred,
                           double &q3_pred) const;

        void getMeasurement(Eigen::Vector3d acceleration, double &q0_meas, double &q1_meas, double &q2_meas, double &q3_meas);

        void getMeasurement(Eigen::Vector3d acceleration, Eigen::Vector3d mag, double &q0_meas, double &q1_meas, double &q2_meas,
                            double &q3_meas);

        void getAccCorrection(Eigen::Vector3d acceleration, double p0, double p1, double p2, double p3, double &dq0, double &dq1,
                              double &dq2, double &dq3);

        void getMagCorrection(double mx, double my, double mz, double p0, double p1, double p2, double p3, double &dq0, double &dq1,
                              double &dq2, double &dq3);

        double getAdaptiveGain(double alpha, const Eigen::Vector3d &acceleration);
    };

    // Utility math functions:

    void normalizeVector(double &x, double &y, double &z);

    void normalizeQuaternion(double &q0, double &q1, double &q2, double &q3);

    void scaleQuaternion(double gain, double &dq0, double &dq1, double &dq2, double &dq3);

    void invertQuaternion(double q0, double q1, double q2, double q3, double &q0_inv, double &q1_inv, double &q2_inv, double &q3_inv);

    void quaternionMultiplication(double p0, double p1, double p2, double p3, double q0, double q1, double q2, double q3, double &r0,
                                  double &r1, double &r2, double &r3);

    void rotateVectorByQuaternion(double x, double y, double z, double q0, double q1, double q2, double q3, double &vx, double &vy,
                                  double &vz);

} // namespace cartographer

#endif // IMU_TOOLS_COMPLEMENTARY_FILTER_H
